#!/usr/bin/env python
# -*- coding: UTF-8 -*-

import rospy
import os,json,cv2
import yaml
import tf
import time
import numpy as np
import math 

from std_msgs.msg import String, Bool
from cln_msgs.msg import Recharge,CleanTask,TaskMeta,MarkerPointMsg,AlarmMsg,VoiceOutput
from cln_msgs.srv import *
from v3_msgs.msg import OnlineTaskCtrl,GoHome,P2PMove
from geometry_msgs.msg import Pose,PoseStamped

# from tool import User_Data_Server

from tool import Log_Parser
log = Log_Parser()

class Navgation_Parser:
    def __init__(self):
        self.__node_name = "Navgation_Parser"
        ##地图名缓存
        self.__map_name = ""
        ## 机器人是否绑定电梯，默认False，没有绑定电梯
        self.__cloudPlatform_bindElevator_flag = False
        self.tracked_pose = None
        self.__language = rospy.get_param("/language", 0)
        
    def InitParser(self, node_name):
        '''
            初始化
        '''
        ## node name配置
        self.__node_name = node_name
        self.__executing_task = CleanTask()   #记录实时任务信息

        self.__docking_task_name =  "charging"
        self.__p2p_task_name =  "p2p_move"

        ## 用户参数
        # self.__user_data_server = User_Data_Server()

        ##发布topic
        self.__robot_recharge_pub = rospy.Publisher("/recharge_trigger", Recharge, queue_size=10)
        self.__task_add_pub = rospy.Publisher("/add_task", CleanTask, queue_size=100)
        self.__alarm_event_pub = rospy.Publisher("/alarm_event", AlarmMsg, queue_size=1)
        self.__voice_pub = rospy.Publisher("/voice_tts",VoiceOutput,queue_size=1)
        self.__gohome_pub = rospy.Publisher("/go_home",GoHome,queue_size=5)
        self.__p2p_pub = rospy.Publisher("/p2p_move",P2PMove,queue_size=5)

        ##订阅topic 
        rospy.Subscriber("/map_name",String,callback=self.MapName_Callback)
        rospy.Subscriber("/task_echo", CleanTask, self.ExecutingTask_Callback,queue_size=1)
        ## 订阅机器人是否绑定梯空 dw.liu 20230704
        rospy.Subscriber("/cloudPlatform_bindElevator", Bool, self.BindElevator_Callback,queue_size=1)       
        rospy.Subscriber("/tracked_pose",PoseStamped,self.__TrackedPose_Callback,queue_size=1)

        ##服务Proxy
        self.__list_marker_client=rospy.ServiceProxy('/marker_point_query',MarkerPoint)
        self.__task_queue_adjust_client = rospy.ServiceProxy("/task_queue_adjust",TaskQueueAdjust)

        #### 查询地图所有标记点（充电桩）dw.liu 20230704
        self.__all_docker_query_client = rospy.ServiceProxy('all_docker_query',AllDockers)

    def GetNodeName(self):
        '''
            获取节点名称
        '''
        return self.__node_name
    #------------------------------

    #--------------------------------------#
    #******#  ROS topic publication #******# 
    #--------------------------------------#
    def PublishRechargeTopic(self,charge_msg):
        '''
            机器人充电指令话题发布
        '''
        self.__robot_recharge_pub.publish(charge_msg)

    def PublishTaskAddTopic(self,clean_task):
        '''
            添加到任务队列话题发布
        '''
        self.__task_add_pub.publish(clean_task)

    #-----------------------------------#
    #******#  ROS topic Callback #******# 
    #-----------------------------------#
    def MapName_Callback(self,msg_data):
        '''
        Topic 获取地图名称
        '''
        self.__map_name = msg_data.data
        log_msg = "[Navgation_Parser]Current loaded map: {}".format(self.__map_name)
        log.LogInfo(self.GetNodeName(),log_msg)

    def ExecutingTask_Callback(self,task):
        """ 
            实时任务执行信息回调函数
        """
        self.__executing_task = task
    
    def BindElevator_Callback(self,msg):
        '''
        订阅机器人是否绑定梯空 dw.liu 20230704
        '''
        self.__cloudPlatform_bindElevator_flag = msg.data

    def __TrackedPose_Callback(self, msg_data):
        """
        机器人位置
        """
        self.tracked_pose = msg_data

    #-----------------------------------#
    #******#  Interface function #******# 
    #-----------------------------------#
    def SetMovetoPoint(self,data):
        '''
            开始运动到目标点
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            if not data : raise Exception("Request data error!")

            target_x = float(data["x"])
            target_y = float(data["y"])
            target_yaw = float(data["yaw"])
            
            #发布导航到点任务，封装任务
            task_p2p = P2PMove()
            task_p2p.option = task_p2p.P2P_MOVE
            task_p2p.goal.position.x = target_x
            task_p2p.goal.position.y = target_y
            task_p2p.goal.position.z = target_yaw
            task_p2p.send_node_name = self.GetNodeName()
            self.__p2p_pub.publish(task_p2p)
            log_msg = "[Navgation_Parser]Craete a new P2P task in map<{}>".format(self.__map_name)
            log.LogInfo(self.GetNodeName(),log_msg)

        except Exception as error:
            rospy.logerr("[Navgation_Parser]Failed to start move to point: {}".format(error))
            ret_dat["code"]=-1
            #ret_dat["ret"]='fail'
            ret_dat["msg"] =str(error)
        finally:
            return ret_dat 

    def CancelMovetoPoint(self):
        '''
            取消运动到目标点
        '''
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
                task_p2p = P2PMove()
                task_p2p.option = task_p2p.CANCEL_P2P_MOVE
                task_p2p.send_node_name = self.GetNodeName()
                self.__p2p_pub.publish(task_p2p)
                
                log_msg = "[Navgation_Parser]Cancel P2P task in map<{}>".format(self.__map_name)
                log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            rospy.logerr("[Hardware_Parser]Failed to cancel move to point: {}".format(error))
            ret_dat["code"]=-1
            #ret_dat["ret"]='fail'
            ret_dat["msg"] =str(error)
        finally:
            return ret_dat 


    def PubLowNodockerState(self,state):
        '''
            发布无充电桩报警
        '''
        alarm_msg = AlarmMsg()
        alarm_msg.node_name = self.__node_name
        alarm_msg.event_time = rospy.Time.now()
        # 该错误码待定
        alarm_msg.event_code = 0X0619
        alarm_msg.event_level = AlarmMsg().LEVEL_3
        alarm_msg.cancel_trigger = False
        event_data=""
        event_code_str=""
        #发布前往默认停靠点报警
        if(state==0):
            if(self.__language==0):
                event_data="前往默认停靠点"
                event_code_str="前往默认停靠点"
            if(self.__language==1):
                event_data="Proceed to the default docking station."
                event_code_str="Proceed to the default docking station."
            if(self.__language==2):
                event_data="前往默认停靠点"
                event_code_str="前往默认停靠点"
            if(self.__language==3):
                event_data="Proceed to the default docking station."
                event_code_str="Proceed to the default docking station."
            if(self.__language==4):
                event_data="Rendez-vous au point d'ancrage par défaut."
                event_code_str="Rendez-vous au point d'ancrage par défaut."
            alarm_msg.event_data = event_data
            alarm_msg.event_code_str = event_code_str
            log_msg = "[Navgation_Parser]  in map<{}> ,前往默认停靠点!".format(self.__map_name)
            log.LogInfo(self.GetNodeName(),log_msg)
        #发布停在原地报警
        if(state==1):
            if(self.__language==0):
                event_data="停在原地"
                event_code_str="停在原地"
            if(self.__language==1):
                event_data="The robot stops in place."
                event_code_str="The robot stops in place."
            if(self.__language==2):
                event_data="停在原地"
                event_code_str="停在原地"
            if(self.__language==3):
                event_data="The robot stops in place."
                event_code_str="The robot stops in place."
            if(self.__language==4):
                event_data="Le robot est stationné sur place."
                event_code_str="Le robot est stationné sur place."
            alarm_msg.event_data = event_data
            alarm_msg.event_code_str = event_code_str
            log_msg = "[Navgation_Parser]  in map<{}> ,停在原地!".format(self.__map_name)
            log.LogInfo(self.GetNodeName(),log_msg)
        #发布前往最近停靠点报警
        if(state==2):
            if(self.__language==0):
                event_data="前往最近的停靠点"
                event_code_str="前往最近的停靠点"
            if(self.__language==1):
                event_data="Proceed to the nearest docking station."
                event_code_str="Proceed to the nearest docking station."
            if(self.__language==2):
                event_data="前往最近的停靠点"
                event_code_str="前往最近的停靠点"
            if(self.__language==3):
                event_data="Proceed to the nearest docking station."
                event_code_str="Proceed to the nearest docking station."
            if(self.__language==4):
                event_data="Aller au point d'ancrage le plus proche."
                event_code_str="Aller au point d'ancrage le plus proche."
            alarm_msg.event_data = event_data
            alarm_msg.event_code_str = event_code_str
            log_msg = "[Navgation_Parser]  in map<{}> ,前往最近的停靠点!".format(self.__map_name)
            log.LogInfo(self.GetNodeName(),log_msg)
        self.__alarm_event_pub.publish(alarm_msg)

    def SetGoHome(self):
        '''
            开始回桩充电
        '''
        log_msg = "[Navgation_Parser]点击回桩"
        log.LogInfo(self.GetNodeName(),log_msg)
        print("点击回桩充电")
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            task_gohome = GoHome()
            task_gohome.option = task_gohome.GO_HOME
            task_gohome.send_node_name = self.GetNodeName()
            self.__gohome_pub.publish(task_gohome)
            log_msg = "[Navgation_Parser] Gohome by user!"
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"]=-1
            ret_dat["msg"] =str(error)
            err_msg = "[Navgation_Parser]Failed to charge: {}".format(str(error))
            log.LogError(self.GetNodeName(),err_msg)  
            rospy.logerr(err_msg)
        finally:
            return ret_dat  

    def CancelGoHome(self):
        '''
            取消回桩
        '''
        log_msg = "[Navgation_Parser]点击回桩"
        log.LogInfo(self.GetNodeName(),log_msg)
        print("点击取消回桩")
        ret_dat={};ret_dat["msg"]='success';ret_dat["code"]=0
        try:
            task_gohome = GoHome()
            task_gohome.option = task_gohome.CANCEL_GO_HOME
            task_gohome.send_node_name = self.GetNodeName()
            self.__gohome_pub.publish(task_gohome)
            log_msg = "[Navgation_Parser] Cancel Gohome by user!"
            log.LogInfo(self.GetNodeName(),log_msg)
        except Exception as error:
            ret_dat["code"]=-1
            ret_dat["msg"] =str(error)
            err_msg = "[Navgation_Parser]Failed to cancel charge: {}".format(str(error))
            log.LogError(self.GetNodeName(),err_msg)  
            rospy.logerr(err_msg)
        finally:
            return ret_dat  

    def SetDockerBack(self):
        '''
            退桩操作
        '''
        return self.CancelGoHome()

